#include "apdu.h"

#include "errors.h"

#include "../apdu.algo/mac.h"
#include "../../mk_lib/mk_logger.h"

#include <stdlib.h>
#include <memory.h>
#include <string.h>

#define GM_APDU_FLAG_TOKEN  0x12

apdu::apdu(void)
{
	init();
}

apdu::apdu( int cse,  unsigned char cla, int ins, int p1, int p2, char * name )
{
	init();
	_cla = (unsigned char ) cla;
	_cse = cse;
	_ins = (unsigned char ) ins;
	_p1 = (unsigned char ) p1;
	_p2 = (unsigned char ) p2;
	
	if(name) strncpy(_name, name, 64);

}

apdu::~apdu(void)
{
	release_resp();
	release_data();
}

void apdu::init()
{
	_cla = 0;
	_cse = 0;
	_ins = 0;
	_p1 = 0;
	_p2 = 0;

	_lc = 0;
	_le = 0;

	_data_ptr = NULL;
	_data_len = 0;

	_resp_ptr = NULL;
	_resp_len = 0;

	_sw1 = 0;
	_sw2 = 0;

	
	_is_data_alloc = 0;

	memset(_name, 0, sizeof(_name));

	_timeout = 0;
}


int apdu::set_lc_data( unsigned char * data, int data_len)
{
	_lc = data_len;
	_data_len = data_len;
	
	release_data();

	_data_ptr = (unsigned char*)malloc(data_len);
	memcpy(_data_ptr, data, data_len);
	_is_data_alloc = 1;

	return 0;
}

int apdu::set_le(int le)
{
	_le = le;
	_resp_len = le;

	return 0;
}
/** Calculates the length of the encoded APDU in octets.*/
int apdu::get_length()
{
	int ret = 4;

	switch (_cse)
	{
	case SC_APDU_CASE_1:
		break;
	case SC_APDU_CASE_2_SHORT:
		ret++;
		break;
	case SC_APDU_CASE_2_EXT:
		ret += 3;
		break;
	case SC_APDU_CASE_3_SHORT:
		ret += 1 + _lc;
		break;
	case SC_APDU_CASE_3_EXT:
		ret += _lc + 3;
		break;
	case SC_APDU_CASE_4_SHORT:
		ret += _lc + 1;
		break;
	case SC_APDU_CASE_4_EXT:
		ret += _lc + 5;
		break;
	default:
		return 0;
	}

	return ret;
}

int apdu::get_sw()
{
	 return _sw1 * 256 + _sw2;
}

int apdu::get_ins()
{
	return _ins;
}

/** Encodes a APDU as an octet string
 *  @param  ctx     sc_context_t object (used for logging)
 *  @param  apdu    APDU to be encoded as an octet string
 *  @param  proto   protocol version to be used
 *  @param  out     output buffer of size outlen.
 *  @param  outlen  size of hte output buffer
 *  @return SC_SUCCESS on success and an error code otherwise
 */
int apdu::apdu2bytes(unsigned char  *out, int outlen)
{
	unsigned char      *p = out;

	int len = get_length();

	if (out == NULL || outlen < len)
		return SC_ERROR_INVALID_ARGUMENTS;

	/* CLA, INS, P1 and P2 */
	*p++ = _cla;
	*p++ = _ins;
	*p++ = _p1;
	*p++ = _p2;

	/* case depend part */
	switch (_cse)
	{
	case SC_APDU_CASE_1:
		break;
	case SC_APDU_CASE_2_SHORT:
		*p = (unsigned char )_le;
		break;
	case SC_APDU_CASE_2_EXT:
		*p++ = (unsigned char )0x00;
		*p++ = (unsigned char )(_le >> 8);
		*p = (unsigned char )_le;
		break;
	case SC_APDU_CASE_3_SHORT:
		*p++ = (unsigned char )_lc;
		memcpy(p, _data_ptr, _lc);
		break;
	case SC_APDU_CASE_3_EXT:
		/* in case of T1 always use 3 bytes for length */
		*p++ = (unsigned char )0x00;
		*p++ = (unsigned char )(_lc >> 8);
		*p++ = (unsigned char )_lc;
		memcpy(p, _data_ptr, _lc);
		break;
	case SC_APDU_CASE_4_SHORT:
		*p++ = (unsigned char )_lc;
		memcpy(p, _data_ptr, _lc);
		p += _lc;
		break;
	case SC_APDU_CASE_4_EXT:
		*p++ = (unsigned char )0x00;
		*p++ = (unsigned char )(_lc >> 8);
		*p++ = (unsigned char )_lc;
		memcpy(p, _data_ptr, _lc);
		p += _lc;
		/* only 2 bytes are use to specify the length of the
		 * expected data */
		*p++ = (unsigned char )(_le >> 8);
		*p = (unsigned char )_le;
		break;
	}

	return SC_SUCCESS;
}

int apdu::get_octets(unsigned char  *buf, int *len)
{
	int	nlen;

	if (buf == NULL || len == NULL)
		return SC_ERROR_INVALID_ARGUMENTS;

	/* get the estimated length of encoded APDU */
	nlen = get_length();
	if (nlen == 0)
		return SC_ERROR_INTERNAL;

	if(*len < nlen)
		return SC_ERROR_BUFFER_TOO_SMALL;

	/* encode the APDU in the buffer */
	if (apdu2bytes(buf, nlen) != SC_SUCCESS)
		return SC_ERROR_INTERNAL;

	*len = nlen;

	return SC_SUCCESS;
}


int apdu::set_resp(const unsigned char  *buf, int len)
{
	if (len < 2)
	{
		/* no SW1 SW2 ... something went terrible wrong */
		DBG_LOGMSG("invalid response: SW1 SW2 missing");
		return SC_ERROR_INTERNAL;
	}
	/* set the SW1 and SW2 status bytes (the last two bytes of
	 * the response */
	_sw1 = (unsigned int)buf[len - 2];
	_sw2 = (unsigned int)buf[len - 1];
	len -= 2;

	_resp_len = len;
	
	release_resp();
	/* set output length and copy the returned data if necessary */
	if(len > 0)
	{
		_resp_len = len;
		_resp_ptr = (unsigned char*)malloc(len);
		memcpy(_resp_ptr, buf, _resp_len);
	}

	return SC_SUCCESS;
}

int apdu::check()
{
	if ((_cse & ~SC_APDU_SHORT_MASK) == 0)
	{
		/* length check for short APDU    */
		if(_cse != SC_APDU_CASE_1)
		{
			if (_le > 256 || (_lc > 255) == 0)
				goto error;
		}
	}
	else if ((_cse & SC_APDU_EXT) != 0)
	{
		/* length check for extended APDU */
		if (_le > 65536 || _lc > 65535)
			goto error;
	}
	else
	{
		goto error;
	}

	switch (_cse & SC_APDU_SHORT_MASK)
	{
	case SC_APDU_CASE_1:
		/* no data is sent or received */
		if (_data_len != 0 || _lc != 0 || _le != 0)
			goto error;
		break;
	case SC_APDU_CASE_2_SHORT:
		/* no data is sent        */
		if (_data_len != 0 || _lc != 0)
			goto error;

		break;
	case SC_APDU_CASE_3_SHORT:
		/* data is sent           */
		if (_data_len == 0 || _data_ptr == NULL || _lc == 0)
			goto error;
		/* no data is expected    */
		if (_le != 0)
			goto error;
		/* inconsistent datalen   */
		if (_data_len != _lc)
			goto error;
		break;
	case SC_APDU_CASE_4_SHORT:
		/* data is sent           */
		if (_data_len == 0 || _data_ptr == NULL || _lc == 0)
			goto error;
		/* data is expected       */

		/* inconsistent datalen   */
		if (_data_len != _lc)
			goto error;
		break;
	default:
		DBG_LOGMSG("Invalid APDU case %d", _cse);
		return SC_ERROR_INVALID_ARGUMENTS;
	}

	return SC_SUCCESS;

error:
	DBG_LOGMSG("Invalid Case %d %s APDU:\n"
		"cse=%02x cla=%02x ins=%02x p1=%02x p2=%02x lc=%lu le=%lu\n"
		"resp=%p resplen=%lu data=%p datalen=%lu",
		_cse & SC_APDU_SHORT_MASK,
		(_cse & SC_APDU_EXT) != 0 ? "extended" : "short",
		_cse, _cla, _ins, _p1, _p2,
		(unsigned long) _lc, (unsigned long) _le,
		_resp_ptr, (unsigned long) _resp_len,
		_data_ptr, (unsigned long) _data_len);
	return SC_ERROR_INVALID_ARGUMENTS;
}

/** Tries to determine the APDU type (short or extended) of the supplied
 *  APDU if one of the SC_APDU_CASE_? types is used.
 *  @param  apdu  APDU object
 */
void apdu::detect_cse()
{
	if (_cse == SC_APDU_CASE_2 || _cse == SC_APDU_CASE_3 ||
	    _cse == SC_APDU_CASE_4)
	{
		int btype = _cse & SC_APDU_SHORT_MASK;
		/* if either Lc or Le is bigger than the maximun for
		 * short APDUs and the card supports extended APDUs
		 * use extended APDUs (unless Lc is greater than
		 * 255 and command chaining is activated) */
		if (_le > 256)
			btype |= SC_APDU_EXT;

		_cse = btype;
	}
}


int apdu::bytes2apdu(const unsigned char  *buf, int len)
{
	const unsigned char *p;
	int len0;

	if (!buf)
		return SC_ERROR_INVALID_ARGUMENTS;

	len0 = len;
	if (len < 4)
	{
		DBG_LOGMSG( "APDU too short (must be at least 4 bytes)");
		return SC_ERROR_INVALID_DATA;
	}

	p = buf;
	_cla = *p++;
	_ins = *p++;
	_p1 = *p++;
	_p2 = *p++;
	len -= 4;

	if (!len)
	{
		_cse = SC_APDU_CASE_1;
		DBG_LOGMSG( "CASE_1 APDU: %lu bytes:\tins=%02x p1=%02x p2=%02x lc=%04x le=%04x",
			(unsigned long) len0, _ins, _p1, _p2, _lc, _le);

		return SC_SUCCESS;
	}

	if (*p == 0 && len >= 3)
	{
		/* ...must be an extended APDU */
		p++;
		if (len == 3)
		{
			_le = (*p++)<<8;
			_le += *p++;
			if (_le == 0)
				_le = 0xffff+1;
			len -= 3;
			_cse = SC_APDU_CASE_2_EXT;
		}
		else
		{
			/* len > 3 */
			_lc = (*p++)<<8;
			_lc += *p++;
			len -= 3;
			if (len < _lc)
			{
				DBG_LOGMSG( "APDU too short (need %lu more bytes)", (unsigned long) _lc - len);
				return SC_ERROR_INVALID_DATA;
			}
			_data_ptr = (unsigned char*) p;
			_data_len = _lc;
			len -= _lc;
			p += _lc;

			if (!len)
			{
				_cse = SC_APDU_CASE_3_EXT;
			}
			else
			{
				/* at this point the apdu has a Lc, so Le is on 2 bytes */
				if (len < 2)
				{
					DBG_LOGMSG("APDU too short (need 2 more bytes)\n");
					return SC_ERROR_INVALID_DATA;
				}
				_le = (*p++)<<8;
				_le += *p++;
				if (_le == 0)
					_le = 0xffff+1;
				len -= 2;
				_cse = SC_APDU_CASE_4_EXT;
			}
		}
	}
	else
	{
		/* ...must be a short APDU */
		if (len == 1)
		{
			_le = *p++;
			if (_le == 0)
				_le = 0xff+1;
			len--;
			_cse = SC_APDU_CASE_2_SHORT;
		}
		else
		{
			_lc = *p++;
			len--;
			if (len < _lc)
			{
				DBG_LOGMSG( "APDU too short (need %lu more bytes)", (unsigned long) _lc - len);
				return SC_ERROR_INVALID_DATA;
			}
			_data_ptr = (unsigned char*) p;
			_data_len = _lc;
			len -= _lc;
			p += _lc;
			if (!len)
			{
				_cse = SC_APDU_CASE_3_SHORT;
			}
			else
			{
				_le = *p++;
				if (_le == 0)
					_le = 0xff+1;
				len--;
				_cse = SC_APDU_CASE_4_SHORT;
			}
		}
	}

	if (len)
	{
		DBG_LOGMSG( "APDU too long (%lu bytes extra)",(unsigned long) len);
		return SC_ERROR_INVALID_DATA;
	}

	DBG_LOGMSG( "Case %d %s APDU, %lu bytes:\tins=%02x p1=%02x p2=%02x lc=%04x le=%04x",
		_cse & SC_APDU_SHORT_MASK,
		(_cse & SC_APDU_EXT) != 0 ? "extended" : "short",
		(unsigned long) len0, _ins, _p1, _p2, _lc, _le);

	return SC_SUCCESS;
}

int apdu::serialize( unsigned char * buf, int * buf_size )
{
	int r = SC_SUCCESS;
	int apdu_size = get_length();

	if(buf == NULL)
	{
		* buf_size = apdu_size;
		return 0;
	}

	/* determine the APDU type if necessary, i.e. to use short or extended APDUs  */
	detect_cse();

	/* basic APDU consistency check */
	r = check();
	if (r != SC_SUCCESS)
		return SC_ERROR_INVALID_ARGUMENTS;
	
	apdu_size = * buf_size;
	unsigned char * apdu_buff = buf;

	r = get_octets(apdu_buff, &apdu_size);
	if(r != SC_SUCCESS)
		return r;

	* buf_size = apdu_size;

	DBG_LOGBUFFER(buf, *buf_size, "[%s]send", _name);

	return r;
}

int apdu::deserialize( unsigned char * rbuf, int rsize )
{
	int r = SC_SUCCESS;

	DBG_LOGBUFFER(rbuf, rsize, "[%s]receive", _name);

	int apdu_size = rsize;
	unsigned char * apdu_buff = rbuf;
	
	r = set_resp(apdu_buff, apdu_size);
	if(r != SC_SUCCESS)
	{
		return r;
	}

	return r;
}


unsigned char * apdu::get_response_data( int * data_len )
{
	* data_len = _resp_len;
	return _resp_ptr;
}


void apdu::release_resp()
{
	if(_resp_ptr) 
	{
		free(_resp_ptr);
		_resp_ptr = NULL;
		_resp_len = 0;
	}
}


void apdu::release_data()
{
	if(_is_data_alloc && _data_ptr != NULL)
	{
		free(_data_ptr);
		_data_ptr = NULL;
		_is_data_alloc = 0;
	}
}



int apdu::gen_apdu_mac(unsigned char key[16], unsigned char iv[16])
{
	unsigned char temp_buff[1024] = {0};
	unsigned char mac[8] = {0};
	int idx = 0;
	
	temp_buff[idx++] = _cla;
	temp_buff[idx++] = _ins;
	temp_buff[idx++] = _p1;
	temp_buff[idx++] = _p2;

	//Correct CASE
	detect_cse();
	
	int new_len = _lc + 4; 
	if(_cse & SC_APDU_EXT)
	{

		temp_buff[idx++]  = (unsigned char )0x00;
		temp_buff[idx++]  = (unsigned char )(new_len >> 8);
		temp_buff[idx++]  = (unsigned char )new_len;
	}
	else
	{
		temp_buff[idx++] = new_len;
	}

	if(_lc > 0)
	{
		memcpy(temp_buff + idx, _data_ptr, _lc);
		idx += _lc;
	}

	sc_mac_gen(key,iv, temp_buff, idx, mac);

	unsigned char * new_data_ptr = (unsigned char*)malloc(_lc + 4);
	memcpy(new_data_ptr, _data_ptr, _lc);
	memcpy(new_data_ptr + _lc, mac, 4);
	
	set_lc_data(new_data_ptr, _lc + 4);

	free(new_data_ptr);

	return 0;
}


int apdu::sc_mac_gen(unsigned char key[16], unsigned char iv[16], 
					 unsigned char * input, int input_len, unsigned char mac[4])
{
	mac_sm4mac_gen(key, iv, input, input_len, mac);
	return 0;
}


int apdu::set_not_last_ins()
{
	_cla |= 0x10;

	return _cla;
}


int apdu::set_last_ins()
{
	_cla &= 0xEF;
	return _cla;
}

apdu * apdu::clone()
{
	apdu * new_apdu = new apdu(_cse, _cla, _ins, _p1, _p2, _name);

	return new_apdu;
}

void apdu::change_case( int cse )
{
	_cse = cse;
}

void apdu::set_comm_timeout(int timeOut)
{
	_timeout = timeOut;
}

int apdu::get_comm_timeout()
{
	return _timeout;
}

